#include "nlink_linktrack_tagframe0.h"
#include "nlink_utils.h"
#include "bsp_usart.h"
#include "common.h"
#include "usart.h"
#include "led.h"

//Read/Write/Demarcate CMD of the Nlink
//读输入帧
uint8_t Read_Input_Frame[128] = \
{
	0x54,0,0x01,0,0,0,0,0,0,0,0,0,0,0xff,0,0xff,0,0,0xff,0xff,0,0xff,0xff,0,0,0,
	0,0,0,0,0,0xff,0xff,0xff,0xff,0xff,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
	0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0x4a
};

#pragma pack(1)
    typedef struct {
        uint8_t header[2];
        uint8_t id;
        uint8_t role;
        NInt24 pos[3];
        NInt24 vel[3];
        NInt24 dis[8];
        float imuGyro[3];
        float imuAcc[3];
        uint8_t reserved1[12];
        int16_t angle[3];
        float q[4];
        uint8_t reserved2[4];
        uint32_t localTime;
        uint32_t systemTime;
        uint8_t reserved3[1];
        uint8_t eop[3];
        uint16_t voltage;
        uint8_t reserved4[5];
        uint8_t checkSum;
    }NLink_LinkTrack_TagFrame0_Raw;
#pragma pack()

static NLink_LinkTrack_TagFrame0_Raw frame_;//模板容器

static uint8_t unpackData(const uint8_t *data,size_t dataLength) 
{
    assert(nltTagFrame0_.kFixedFrameLength == sizeof (frame_));//判断传入参数是否合法
    if(dataLength < nltTagFrame0_.kFixedFrameLength || data[0] != nltTagFrame0_.kFrameHeader || data[1] != nltTagFrame0_.kFunctionMark) return 0;//判断帧长，帧头，功能字
    if(!verifyCheckSum(data,nltTagFrame0_.kFixedFrameLength)) return 0;//校验

    memcpy(&frame_, data, nltTagFrame0_.kFixedFrameLength);//内存复制，将传进来的数据装进模板容器
    nltTagFrame0_.data.role =frame_.role;
    nltTagFrame0_.data.id =frame_.id;
    nltTagFrame0_.data.localTime =frame_.localTime;
    nltTagFrame0_.data.systemTime =frame_.systemTime;
    nltTagFrame0_.data.voltage = frame_.voltage / kVoltageMultiply_;

    TRANSFORM_ARRAY_INT24(nltTagFrame0_.data.pos,frame_.pos,kPosMultiply_)
    TRANSFORM_ARRAY_INT24(nltTagFrame0_.data.vel,frame_.vel,kVelMultiply_)
    TRANSFORM_ARRAY_INT24(nltTagFrame0_.data.dis,frame_.dis,kDisMultiply_)
    TRANSFORM_ARRAY(nltTagFrame0_.data.imuGyro,frame_.imuGyro,1)
    TRANSFORM_ARRAY(nltTagFrame0_.data.imuAcc,frame_.imuAcc,1)
    TRANSFORM_ARRAY(nltTagFrame0_.data.q,frame_.q,1)
    TRANSFORM_ARRAY(nltTagFrame0_.data.angle,frame_.angle,kAngleMultiply_)
    TRANSFORM_ARRAY(nltTagFrame0_.data.eop,frame_.eop,kEopMultiply_)

    return 1;
}

NLink_LinkTrack_TagFrame0 nltTagFrame0_ = {
    .kFixedFrameLength = 128,
    .kFrameHeader = 0x55,
    .kFunctionMark = 0x01,
    .unpackData = unpackData
};

void test(void)
{
//	u1_printf("%d",sizeof(frame_));
	u1_printf("id:%d role:%d pos:%.3f vel:%.3f dis:%.3f imuGyro:%.3f imuAcc:%.3f angle:%.3f\r\n",nltTagFrame0_.data.id,nltTagFrame0_.data.role,nltTagFrame0_.data.pos[0],nltTagFrame0_.data.vel[0],\
	nltTagFrame0_.data.dis[0],nltTagFrame0_.data.imuGyro[0],nltTagFrame0_.data.imuAcc[0],nltTagFrame0_.data.angle[0]);
}

void send_Read_Input_Frame(void)
{
	do
	{
		delay_ms(1000);
		send_data_dma_u3(Read_Input_Frame,128);
		delay_ms(5);
	}while(USART3_RX_BUF[0] != 0x54);//发送读输入帧，等待读输入返回帧
//	delay_ms(1000);
//	send_data_dma_u3(Read_Input_Frame,128);
//	delay_ms(5);
}

void send_demarcate(void)
{
	/*生成一键标定指令*/
	Read_Input_Frame[2]=0x02;
	Read_Input_Frame[17]=0x08;
	updateCheckSum(Read_Input_Frame,128);
	
	send_data_dma_u3(Read_Input_Frame,128);//发送一键标定指令
	while(USART3_RX_BUF[0]!=0x54);//等待指令发送完成
	//delay_ms(1000);
	
	/*生成一键标定状态读取指令*/
	Read_Input_Frame[2]=0x20;
	Read_Input_Frame[17]=0x08;
	updateCheckSum(Read_Input_Frame,128);
	
	do
	{
		send_data_dma_u3(Read_Input_Frame,128);//发送一键标定状态读取指令
		delay_ms(200);
	}while(USART3_RX_BUF[17]!=0x00);//判断是否标定完成
	
	Demarcate_Succ();
}

void Demarcate_Succ(void)
{
	BEEP_ON();
	delay_ms(100);
	BEEP_OFF();
	delay_ms(100);
	BEEP_ON();
	delay_ms(100);
	BEEP_OFF();
}
